//#include <TimeAlarms.h>
#include <IRremote.h>
#include <Servo.h> 

#define SERVO_PIN 9
#define IR_RECEIVE_PIN 11
#define IR_SEND_PIN 3
#define M1_SPEED 6
#define M1_DIRECTION 7
#define M2_SPEED 5
#define M2_DIRECTION 4
#define BOOM_PIN 8

Servo servo;  // create servo object to control a servo 
                // a maximum of eight servo objects can be created 

unsigned long last_command=-1;

class Motor{
  private: 
    int _speed;
    int _direction;

  public:
  Motor(int speed,int direction){
    _speed = speed;
    _direction = direction;
  }
  
  
  void init(){
   pinMode(_direction, OUTPUT); // настраиваем выводы
   analogWrite(_speed, 0);
  }
  
  void forward(int spd){
    digitalWrite(_direction, HIGH);
    analogWrite(_speed, spd);
  }
  
  void back(int spd) // первый назад
  {
    digitalWrite(_direction, LOW);
    analogWrite(_speed, spd);
  }
  
  void stop() // первый назад
  {
    analogWrite(_speed, 0);
  }
};

struct Remote
{
  unsigned const long UP;
  unsigned const long DOWN;
  unsigned const long RIGHT;
  unsigned const long LEFT;
  unsigned const long LONG_UP;
  unsigned const long LONG_DOWN;
  unsigned const long STOP;
  unsigned const long TURREL_L;
  unsigned const long TURREL_R;
  unsigned const long REPEAT_CODE;
  unsigned const long TURREL_FIRE;
};


Remote uni = {
0xE99D556D, 
0x7686B86B, 
0xEB9D5893, 
0xDCC262AA, 
0x7486B545, 
0x230E4D82,
0x8C22657B,
0x35A9425F,
0xF377C5B7,
0x50C5D1F,
0x9EF4941F};

Motor motor1(M1_SPEED,M1_DIRECTION);
Motor motor2(M2_SPEED,M2_DIRECTION);

int fspd = 0;
int side_spd = 0;

IRsend irsend;
IRrecv irrecv(IR_RECEIVE_PIN);
decode_results results;

void setup()
{
  pinMode(BOOM_PIN, OUTPUT);
  servo.attach(SERVO_PIN);
  servo.write(90); 
  motor1.init();
  motor2.init();

  irrecv.enableIRIn(); // Start the receiver
//  LedOn(BOOM_PIN);
  LedOff(BOOM_PIN);
  Serial.begin(9600);
  Serial.println("Setup completed...");
}


void loop() {
  boolean send_ir = false;
  if (irrecv.decode(&results)) {
    boolean isDoomed= (results.decode_type == SONY) && (results.value == 0xABC0);
//    boolean isDoomed= (results.decode_type == NEC) && (results.value == 0xFFB847);
    
    if(isDoomed){
      Serial.println("DESTROID!!!");
      robot_stop();
      for (int i=0; i<5; i++) {
        LedOn(BOOM_PIN);
        delay(100);
        LedOff(BOOM_PIN);
        delay(100);
      }
      
      LedOn(BOOM_PIN);
      delay(5000);
      LedOff(BOOM_PIN);
    }
    
    Serial.print("'real' decode: ");
    Serial.print(results.value, HEX);
    Serial.println(", hash decode: ");
    unsigned long code = decodeHash(&results);
    if(code == uni.REPEAT_CODE){
      code = last_command;
    }else{
      last_command = code;
    }
    
    if((code == uni.UP)||(code == uni.LONG_UP))
      {
        if((fspd < 75) && (fspd > -75)){
          fspd = 75;
        }else{
          fspd=fspd+15;
        }
        
        Serial.print("up ");
        Serial.println(fspd, DEC);
      }
      else if((code == uni.DOWN) || (code == uni.LONG_DOWN))
      {
        if((fspd < 75) && (fspd > -75)){
          fspd = -75;
        }else{
          fspd=fspd-15;
        }

        Serial.print("down ");
        Serial.println(fspd, DEC);
      }
      else if(code == uni.RIGHT){
        Serial.println("right");
        side_spd = side_spd+1;
      }
      else if(code == uni.LEFT){
        Serial.println("left");
        side_spd = side_spd-1;
      }
      else if(code == uni.TURREL_L){
        Serial.println("TURREL L");
        rotate(-15);
      }
      else if(code == uni.TURREL_R){
        Serial.println("TURREL R");
        rotate(15);
      }
      else if(code == uni.STOP){
        Serial.println("STOP");
        robot_stop();
      }
      else if(code == uni.TURREL_FIRE){
        Serial.println("FIRE");
        send_ir = true;
      }
      else{
        Serial.println(code, HEX);    
        last_command = -1;
      }
        
    irrecv.resume(); // Resume decoding (necessary!)
  }

  motor_speed();
  motor_rotate();
  if(send_ir == true){
    canon_fire();
  }
  
  delay(50);  
}

void robot_stop(){
    fspd=0;
    side_spd = 0;
}

void canon_fire(){
    Serial.print("Fire!");
    irsend.sendSony(0xABC1, 16);
    irrecv.enableIRIn();
    for (int i=0; i<2; i++) {
        LedOn(BOOM_PIN);
        delay(100);
        LedOff(BOOM_PIN);
        delay(100);
    }
}

void motor_speed(){
  
  if(fspd < -254) fspd = -254;
  if(fspd > 254) fspd = 254;
 
  if(fspd >= 0){
    motor1.forward(fspd);
  }else{
    motor1.back(fspd*(-1));
  }
}

void motor_rotate(){
  if(side_spd < -1) side_spd = -1;
  if(side_spd > 1) side_spd = 1;
  
  if(side_spd == 1){
    motor2.forward(254);
  }
  if (side_spd == -1){
    motor2.back(254);
  }
  if(side_spd == 0){
    motor2.forward(0);
  }
}

// Compare two tick values, returning 0 if newval is shorter,
// 1 if newval is equal, and 2 if newval is longer
// Use a tolerance of 20%
int compare(unsigned int oldval, unsigned int newval) {
  if (newval < oldval * .8) {
    return 0;
  } 
  else if (oldval < newval * .8) {
    return 2;
  } 
  else {
    return 1;
  }
}

// Use FNV hash algorithm: http://isthe.com/chongo/tech/comp/fnv/#FNV-param
#define FNV_PRIME_32 16777619
#define FNV_BASIS_32 2166136261

/* Converts the raw code values into a 32-bit hash code.
 * Hopefully this code is unique for each button.
 */
unsigned long decodeHash(decode_results *results) {
  unsigned long hash = FNV_BASIS_32;
  for (int i = 1; i+2 < results->rawlen; i++) {
    int value =  compare(results->rawbuf[i], results->rawbuf[i+2]);
    // Add value into the hash
    hash = (hash * FNV_PRIME_32) ^ value;
  }
  return hash;
}

void LedOn(int pinNumber){
  digitalWrite(pinNumber, HIGH);
}


void LedOff(int pinNumber){
  digitalWrite(pinNumber, LOW);
}

void rotate(int grad){
  int rotate_speed = -1;
  int grad_module = -1;
  
  if(grad < 0) {
    rotate_speed = 0;
    grad_module = -grad;
  }
  else {
    rotate_speed = 180;
    grad_module = grad;
  }
    
  long num = ((long)grad_module)*((long)1000);
  Serial.print("rotate:");
  Serial.println(num, DEC);
  int ms = (int) ((num)/(43*360/60));
  servo.write(rotate_speed);
  delay(ms);
  servo.write(90);
}
